/*========================================================================================================================================================================================================================
 * INCLUDES==============================================================================================================================================================================================================
 */

#include "HybridSystemController.h"

#include <LiquidCrystal.h>
#include <Servo.h>
#include <pt.h>
#include <ServoRate.h>

/*========================================================================================================================================================================================================================
 * DEFINES==============================================================================================================================================================================================================
 */

#define PIN_SERVO_THROTTLE 9
#define DEGREE_THROTTLE_LOW 60
#define RATE_THROTTLE_LOW 0
#define DEGREE_THROTTLE_HIGH 85
#define RATE_THROTTLE_HIGH 20
#define PIN_SERVO_CLUTCH 10
#define DEGREE_CLUTCH_DOWN 0
#define RATE_CLUTCH_DOWN 60
#define DEGREE_CLUTCH_MIDDLE 90
#define DEGREE_CLUTCH_UP 180
#define RATE_CLUTCH_UP 0
#define SECONDS_FULL_THROTTLE 5
#define LOAD_WHEEL_MIN_RPM 50
#define BUTTON_DEBOUNCE 100

#define SPEED_DRIVE_HIGH 255
#define ACC_DRIVE_UP_TO_HIGH 7843
#define SPEED_DRIVED_OFF 0
#define DEC_DRIVE_OFF 0
#define SPEED_DRIVE_LOW 20
#define ACC_DRIVE_UP_TO_LOW 100000

#define PIN_MOTOR_DRIVE 6

#define PIN_HYBRID_BATTERY A0

#define HYBRID_MODE_ACCELERATE 1
#define HYBRID_MODE_HOLD_SPEED 2
#define HYBRID_MODE_COAST 3

LiquidCrystal lcd( 12, 11, 5, 4, 3, 7 );

double getVoltage( struct VoltageMonitor & voltageMonitor );
double avgSensorReadings( struct VoltageMonitor & voltageMonitor );
void motorSetSpeed( struct Motor & motor, int nTargetSpeed, unsigned long nAccRateInMicrosPerAcc );

/*========================================================================================================================================================================================================================
 * VARIABLES==============================================================================================================================================================================================================
 */

int nHybridMode = 0;

struct Motor
{
  struct pt pt;
  struct TsMicroDelay tsMicroDelay;
  int nPin;
  int nCurrSpeed;
  int nTargetSpeed;
  unsigned long nAccRateInMicrosPerAcc;
  unsigned long nLastAccTimestampInMicros;
};

struct Tach
{
  unsigned long nMillisLast;
  unsigned long nMillisCurrent;
  unsigned int nTachDebounce;
};

struct VoltageMonitor
{
  struct pt pt;
  struct TsMicroDelay tsMicroDelay;
  boolean bCheckedThisCoast;
  int nPin;
  int nSecondsBetweenVoltageChecks;
  int nPinMin;
  int nPinMax;
  int nVoltsMin;
  int nVoltsMax;
  int nSensorVal;
  double nVolts;
  double nVoltsTest1;
  double nVoltsTest2;
  double nVoltsTest3;
  double nVoltsTest4;
  double nVoltsTest5;
  double nVoltsTest6;
  double nVoltsTest7;
  double nVoltsTest8;
  double nVoltsTest9;
  double nVoltsTest10;
};

struct TsServo servoThrottle;
struct TsServo servoClutch;
struct TsMicroDelay tsMicroDelay;
struct Motor motorDrive;
struct Tach tachLoadWheel;
struct VoltageMonitor hybridSystemBattery;
struct pt ptStartEngine;
struct pt ptThreadKeepCharged;
struct pt ptMotorController;

/*========================================================================================================================================================================================================================
 * PROTOTHREADS==============================================================================================================================================================================================================
 */

// Local variables problematic in protothreads, so the following macro is used to put the locals in a place where they aren't messed with by the prototrhread
#define __MOTOR_POS_NEXT(motor, nDiffFromTarget) (motor.nCurrSpeed) + ( ((nDiffFromTarget) > 0) ? 1 : -1 )
#define __MOTOR_DELAY_MICROS(motor) ( ( (motor.nAccRateInMicrosPerAcc) > 0 ) ? (motor.nAccRateInMicrosPerAcc) : 0 )
#define __MOTOR_ELAPSED_TIME(motor) ( micros() - motor.nAccRateInMicrosPerAcc )
#define __MOTOR_DELAY_MICROS_LESS_ELAPSED_TIME(servo) ( (__MOTOR_DELAY_MICROS(motor) > __MOTOR_ELAPSED_TIME(motor)) ? (__MOTOR_DELAY_MICROS(motor) - __MOTOR_ELAPSED_TIME(motor)) : 0  )
PT_THREAD( threadMotorController( struct Motor & motor ) )
{
  int nDiff = (int) motor.nTargetSpeed - (int) motor.nCurrSpeed;

  PT_BEGIN( &motor.pt );

  if ( nDiff != 0 )
  {
    analogWrite(motor.nPin, __MOTOR_POS_NEXT(motor, nDiff));
    motor.nCurrSpeed = __MOTOR_POS_NEXT(motor, nDiff);
    if ( ( motor.nAccRateInMicrosPerAcc > 0 ) && ( motor.nTargetSpeed != motor.nCurrSpeed ) )
    {
      PT_SPAWN( &motor.pt, &motor.tsMicroDelay.pt, threadMicroDelay(motor.tsMicroDelay, __MOTOR_DELAY_MICROS_LESS_ELAPSED_TIME(Motor) ) );
    }
  }

  PT_END( &motor.pt );
  return PT_ENDED;
}


PT_THREAD( startEngine( struct pt & pt ) )
{
  PT_BEGIN(&pt);

  //insert the engine starting sequence here

  PT_END(&pt);
  return PT_ENDED;
}


PT_THREAD( threadVoltageMonitor( struct VoltageMonitor & voltageMonitor ) )
{
  PT_BEGIN(&voltageMonitor.pt);

  if( (nHybridMode == HYBRID_MODE_COAST) && (voltageMonitor.bCheckedThisCoast == false) )
  {
    voltageMonitor.nVoltsTest1 = getVoltage( voltageMonitor );
    PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
    voltageMonitor.nVoltsTest2 = getVoltage( voltageMonitor );
    PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
    voltageMonitor.nVoltsTest3 = getVoltage( voltageMonitor );
    PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
    voltageMonitor.nVoltsTest4 = getVoltage( voltageMonitor );
    PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
    voltageMonitor.nVoltsTest5 = getVoltage( voltageMonitor );
    PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
    voltageMonitor.nVoltsTest6 = getVoltage( voltageMonitor );
    PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
    voltageMonitor.nVoltsTest7 = getVoltage( voltageMonitor );
    PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
    voltageMonitor.nVoltsTest8 = getVoltage( voltageMonitor );
    PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
    voltageMonitor.nVoltsTest9 = getVoltage( voltageMonitor );
    PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
    voltageMonitor.nVoltsTest10 = getVoltage( voltageMonitor );
    voltageMonitor.nVolts = avgSensorReadings( voltageMonitor );
    voltageMonitor.bCheckedThisCoast = true;
  }

  PT_END(&voltageMonitor.pt);
  return PT_ENDED;
}


PT_THREAD( threadDriveController( struct Motor & motor ) )
{
  PT_BEGIN(&motor.pt);

  // Step one: move clutch down until driveshaft begins to touch load wheel
  lcd.clear();
  lcd.print( "Step One" );
  servoSetPosition( servoClutch, DEGREE_CLUTCH_DOWN, RATE_CLUTCH_DOWN );
  PT_WAIT_UNTIL( &motor.pt, servoGetPosition(servoClutch) == DEGREE_CLUTCH_MIDDLE );

  // Step two: begin to speed up motor while continuing to lower clutch
  lcd.clear();
  lcd.print( "Step Two" );
  motorSetSpeed( motor, SPEED_DRIVE_HIGH, ACC_DRIVE_UP_TO_HIGH );
  PT_WAIT_UNTIL( &motor.pt, servoGetPosition(servoThrottle) == DEGREE_THROTTLE_HIGH );

  // Step three: wait for a bit
  lcd.clear();
  lcd.print( "Step Three" );
  PT_SPAWN( &motor.pt, &tsMicroDelay.pt, threadMicroDelay(tsMicroDelay, SECONDS_FULL_THROTTLE * 1000000) );

  // Step four: raise clutch and turn off motor
  lcd.clear();
  lcd.print( "Step Four" );
  servoSetPosition( servoClutch, DEGREE_CLUTCH_UP, RATE_CLUTCH_UP );
  PT_WAIT_UNTIL( &motor.pt, servoGetPosition(servoClutch) == DEGREE_CLUTCH_MIDDLE );  // TODO: change to get drive speed
  motorSetSpeed( motor, SPEED_DRIVED_OFF, DEC_DRIVE_OFF );

  // Step five: wait until load wheel slows down
  lcd.clear();
  lcd.print( "Step Five" );
  PT_WAIT_UNTIL( &motor.pt, tachGetRpm(tachLoadWheel) <= LOAD_WHEEL_MIN_RPM );

  // Step six: raise motor speed to SPEED_DRIVE_LOW
  motorSetSpeed( motor, SPEED_DRIVE_LOW, ACC_DRIVE_UP_TO_LOW );


  PT_END(&motor.pt);
  return PT_ENDED;
}


PT_THREAD( threadKeepCharged( struct pt & pt ) )
{
  PT_BEGIN(&pt);

  //insert battery and generator logic here

  PT_END(&pt);
  return PT_ENDED;
}

/*========================================================================================================================================================================================================================
 * FUNCTIONS==============================================================================================================================================================================================================
 */

void motorInit( struct Motor &motor )
{
  PT_INIT( &motor.pt );
}

void tachInit( struct Tach &tach, int nMillisLastInit, int nMillisCurrentInit )
{
  tach.nMillisLast = nMillisLastInit;
  tach.nMillisCurrent = nMillisCurrentInit;
}

//void voltageMonitorInit( struct voltageMonitor &voltageMonitor,  )

void tachISR()
{
  if ( ( millis() - tachLoadWheel.nMillisCurrent ) >= tachLoadWheel.nTachDebounce )
  {
    tachLoadWheel.nMillisLast = tachLoadWheel.nMillisCurrent;
    tachLoadWheel.nMillisCurrent = millis();
  }
}

int tachGetRpm( struct Tach &tach )
{
  unsigned long nDiff = ( tach.nMillisCurrent - tach.nMillisLast );
  return ( 60000 / nDiff );
}

void motorSetPosition( struct Motor & motor, int nTargetSpeed, unsigned long nAccRateInMicrosPerAcc )
{
  motor.nTargetSpeed = nTargetSpeed;
  motor.nAccRateInMicrosPerAcc = nAccRateInMicrosPerAcc;
}

double getVoltage( struct VoltageMonitor & voltageMonitor )
{
  voltageMonitor.nSensorVal = analogRead( voltageMonitor.nPin );
  return ( ( (double) (voltageMonitor.nVoltsMax - voltageMonitor.nVoltsMin) / (double) (voltageMonitor.nPinMax - voltageMonitor.nPinMin) ) * (double) voltageMonitor.nSensorVal );
}

double avgSensorReadings( struct VoltageMonitor & voltageMonitor )
{
  return ( (
      voltageMonitor.nVoltsTest1 +
      voltageMonitor.nVoltsTest2 +
      voltageMonitor.nVoltsTest3 +
      voltageMonitor.nVoltsTest4 +
      voltageMonitor.nVoltsTest5 +
      voltageMonitor.nVoltsTest6 +
      voltageMonitor.nVoltsTest7 +
      voltageMonitor.nVoltsTest8 +
      voltageMonitor.nVoltsTest9 +
      voltageMonitor.nVoltsTest10)
      / 10 );
}

void motorSetSpeed( struct Motor & motor, int nTargetSpeed, unsigned long nAccRateInMicrosPerAcc )
{
  motor.nTargetSpeed = nTargetSpeed;
  motor.nAccRateInMicrosPerAcc = nAccRateInMicrosPerAcc;
}

/*========================================================================================================================================================================================================================
 * SETUP/LOOP==============================================================================================================================================================================================================
 */

void setup()
{
  lcd.begin( 16, 2 );
  lcd.print( "Initializing..." );

  motorInit( motorDrive );

  attachInterrupt( 0, tachISR, FALLING );
  digitalWrite( 2, HIGH );
  tachInit( tachLoadWheel, 0, 0 );

  PT_INIT( &ptStartEngine );
  PT_INIT( &ptThreadKeepCharged );

  servoInit( servoThrottle, PIN_SERVO_THROTTLE, DELAY_SERVO_AIRTRONICS_94102, DEGREE_THROTTLE_LOW, 0 );
}

void loop()
{
  threadServoController( servoThrottle );
  threadMotorController( motorDrive );
  threadDriveController( motorDrive );
  threadKeepCharged( ptThreadKeepCharged );
}
